#!/usr/bin/python
# -*- coding: utf-8 -*-
import threading
import platform
import cv2
import time
import atexit
from log_info import logger
from infer_exe import Client
from vehicle import MecanumBase
from arm import Arm
from camera import Camera
from box import Box
from sensor import Sensor
from rotor import Rotor
from tools import pas
from tools import kill_other_python
from tools import string_to_chunks


class MyCar(MecanumBase):
    def __init__(self, task_dict=None, speed=0.15):  # 属性初始化
        self.start_time = time.time()  # 获取小车创建的时间,用于日志
        kill_other_python()  # 杀死其他python进程
        super(MyCar, self).__init__()  # 调用继承的初始化
        # ----------------------------------------------------------------
        self.cruse = Client("lane")  # 初始化巡航模型
        self.det_task = Client("det_task")  # 初始化任务识别模型
        self.arm = Arm(self)  # 初始化机械臂
        self.box = Box(self)  # 初始化物品盒
        self.sensor = Sensor(self)  # 初始化超声波传感器
        self.rotor = Rotor(self)  # 初始化转子
        # 初始化摄像头
        if 'Windows' in platform.platform():
            self.front_camera = Camera(0)
            self.arm_camera = Camera(2)
        else:
            self.front_camera = Camera("video-front")
            self.arm_camera = Camera("video-arm")
        atexit.register(self.end)  # 析构函数,清理后台
        # ----------------------------------------------------------------
        self.state = "wait"  # 初始状态变成等待
        self.coe = 1.2
        self.end = False
        self.which = "01"
        self.speed = speed
        self.stop_det_mode = False
        self.wait = None  # 用于自定义任务结束后任务监视器休眠时间
        self.det_direct = 1  # 表明现在是向右的检测摄像头
        self.task_class = None  # 先预设个任务类型变量
        self.task_dict = task_dict  # 任务清单存储任务和对应函数
        self.monitoring_pause = False  # 检测任务暂停标志
        self.stop = False  # 巡航是否停止的标志
        self.task_monitor_over = False  # 不停止任务检测
        self.pas_task = ['left', 'right', 'red_block', 'blue_block', 'house', 'yellow_ball',
                         'pull', 'cylinder_h', 'cylinder_l', 'cylinder_m']  # 不会被模型认出的对象
        self.label_list = ['board', 'left', 'right', 'red_block', 'blue_block', 'house', 'blue_ball', 'yellow_ball',
                           'pole', 'tower', 'pull', 'cylinder_h', 'cylinder_l', 'cylinder_m']  # 模型可识别的对象
        self.task_list = ['house', 'blue_ball', 'pole', 'tower', 'pull', 'board']  # 按顺序排列的任务列表
        self.index = -1  # 当前任务序号,每次执行任务加一
        # ---------------------------------------------------------------
        self.init()  # 模型初始化

    def init(self):  # 等待模型初始化
        self.sensor.init()
        self.cruse.init()
        self.det_task.init()
        self.beep()
        logger.info("智能车初始化成功,耗时 {} s".format(time.time() - self.start_time))  # 小车初始化成功

    def start(self):
        self.start_time = time.time()  # 用于计算全程用时
        self.lane_distance(distance=0.4, axis=0, speed=0.1)  # 先走到红方块处
        self.arm.do_task("det_pos_right_plus", wait=0.2)  # 伸展结构

        self.blind_finish_task()  # 抓取两个物块,因为和出发点很近不使用定位

        task = threading.Thread(target=self.task_monitor, args=(10,))  # 开始任务监视线程
        task.setDaemon(True)
        task.start()

        lane_mode = threading.Thread(target=self.lane_mode)
        lane_mode.setDaemon(True)
        lane_mode.start()

        self.state = "lane"  # 进入巡航状态
        self.stop = False  # 巡航停止标志取消
        self.logout("启动初始化完成,巡航开始")
        self.lane_base()  # 开始巡航

    def end(self):
        self.logout(f"耗时{time.time() - self.start_time}s")
        self.logout("停下程序,正在清理资源...")
        self.end = True
        self.stop_det_mode = True
        self.task_monitor_over = True
        self.sensor.close()
        self.front_camera.close()
        self.arm_camera.close()
        self.det_task.close()
        self.cruse.close()
        kill_other_python()
        self.logout(f"程序结束!!")
        self.close()

    def lane_base(self):
        while True:
            if self.end:
                break
            if self.stop:  # 如果停止标志改变就去做任务
                self.do_task()
            # self.logout("当前没有任务,继续巡航")
            self.auto_lane(self.speed)

    # 识别任务
    def figure_task_by_model(self):
        img = self.arm_camera.read()  # 读取机械臂摄像头图片
        img = cv2.resize(img, (416, 416))
        res = self.det_task(img, timeout=10)  # 获取识别结果
        task_class = None
        if res is None:  # 如果没有结果就返回None
            return task_class
        max_score = 0
        for bbox in res:
            task, score = self.label_list[int(bbox[0])], bbox[1]
            if score < 0.55:  # 置信度不高的过滤掉
                continue
            if task in self.pas_task:
                continue
            if score > max_score:
                task_class = task
                max_score = score
        return task_class

    def figure_task_by_index(self):
        self.index += 1
        task_class = self.task_list[self.index]
        return task_class

    # 执行任务
    def do_task(self):
        self.state = "task"  # 进入任务状态
        self.mecanum_wheel(0, 0, 0)  # 立马停下
        self.task_class = self.figure_task_by_index()  # 识别任务
        task_fun = self.task_dict[self.task_class]  # 获取任务函数
        self.wait = task_fun()  # 然后再运行任务函数
        self.task_over()  # 任务结束的后续处理

    def task_over(self):
        t = threading.Thread(target=self.task_monitor_sleep,
                             args=(self.wait if self.wait is not None else 3,))  # 正常每个任务之间休眠3s
        t.start()  # 传感器休眠
        self.logout(f"任务{self.task_class}正在结束")
        self.task_dict[self.task_class] = pas  # 任务执行结束后把他从任务清单中去除
        self.pas_task.append(self.task_class)  # 识别过的任务不再识别
        self.task_class = None  # 刷新变量
        self.stop = False  # 停止巡航的标志取消
        self.state = "lane"  # 返回巡航

    # 按照要求的距离平移一段,axis=0表示x轴也就是前后移动,axis=1表示y轴也就是水平移动
    def lane_distance(self, distance, axis=0, speed=0.02):
        start_place = self.get_odom()
        ind = 1 if distance > 0 else -1
        while True:
            # print(self.get_odom())
            if ind * self.get_odom()[axis] >= ind * (start_place[axis] + distance):
                break
            self.mecanum_wheel(speed * ind * abs(axis - 1), speed * ind * abs(axis), 0)

        self.mecanum_wheel(0, 0, 0)
        direct = {"01": "向前",
                  "0-1": "向后",
                  "11": "水平向右",
                  "1-1": "水平向左"}
        self.logout(f"小车 {direct[str(axis) + str(ind)]} 行驶了{distance}m")

    # 巡航
    def auto_lane(self, speed=0.15):
        img = self.front_camera.read()  # 读取前方相机内容
        y_speed, angle_speed = self.cruse(img)  # 得到模型的输出
        self.mecanum_wheel(speed, y_speed, angle_speed * self.coe)  # 将输出作用在巡航上
        self.logout([float(str(y_speed)[:9]), float(str(angle_speed)[:9])])

    # 按照要求的距离向前自动巡航
    def auto_lane_distance(self, distance, speed=0.1):
        start_place = self.get_dis_traveled()
        ind = 1 if distance > 0 else -1
        while True:
            if ind * self.get_dis_traveled() >= ind * (start_place + distance):
                break
            self.auto_lane(speed)
        self.mecanum_wheel(0, 0, 0)
        direct = {"1": "向前", "-1": "向后"}
        self.logout(f"小车 {direct[str(ind)]} 巡航了{distance}m")

    def task_monitor_sleep(self, wait=2.5):
        self.logout(f"任务监视器暂停")
        self.monitoring_pause = True
        time.sleep(wait)
        self.wait = None  # 重置
        self.monitoring_pause = False
        self.logout("任务监视器启动")

    # 任务监视器
    def task_monitor(self, gap=10):
        time.sleep(gap)  # 这里相当于第一个抓物块的任务完成了,让任务检测检测休眠一段时间,防止卡在原地
        self.logout("开启任务监听线程")
        while True:
            if self.task_monitor_over:
                break
            if self.monitoring_pause:  # 通过标志暂停监视器
                continue
            self.sensor.direct = "right" if self.det_direct == 1 else "left"  # 根据self.det_direct改变传感器的端口
            if (self.sensor.res[1] == 0 and self.sensor.res[0] < 25) and not self.monitoring_pause:  # 如果传感器表明某侧障碍物距离足够近,就是到达了任务点
                self.stop = True
                self.monitoring_pause = True

    def dis_monitor(self, distance):  # 通过历程监视任务点是否到达
        start_distance = self.get_dis_traveled()
        while True:
            if self.get_dis_traveled() >= start_distance + distance:
                self.to_goal = True
                break

    # 根据任务检测模型调整前后
    def adjust_pos_x(self, x_min, x_max, timeout=4, speed=0.02):
        self.mecanum_wheel(0, 0, 0)
        self.logout("车身停止,开始调正前后位置")
        # self.det_task.server.settimeout(timeout)
        n = 0
        # 根据图像识别调整前后位置
        while True:
            img = self.arm_camera.read()
            res = self.det_task(img)
            if res:  # 如果有结果
                if n >= timeout:
                    break
                self.task_class, dis = self.get_closet_task(res)
                if self.task_class is None:
                    self.lane_distance(distance=0.03, axis=0)
                    n += 1
                    continue
                if dis < x_min or dis > x_max:  # 如果距离不在阈值内
                    self.lane_distance(distance=dis * 0.0005 * 1.4, speed=speed)  # 就调整位置
                    n += 1
                    continue
                break

    # 根据传感器调整左右
    def adjust_pos_y(self, y_min, y_max, timeout=5, speed=0.02):
        self.mecanum_wheel(0, 0, 0)
        self.logout("车身停止,开始调正水平位置")
        # self.det_task.server.settimeout(timeout)

        m = (y_min + y_max) / 2
        n = 0
        while True:
            dis = self.sensor.res[0] - m
            if dis > 5:
                dis = 5
            if y_max >= self.sensor.res[0] >= y_min:
                break
            if n >= timeout:
                break
            self.lane_distance(distance=(dis / 1000) * self.det_direct, axis=1, speed=speed)  # 根据超声波调整水平位置
            n += 1

    # 根据巡航模型调整位置角度
    def adjust_pos_omega(self, ang_min, ang_max, timeout=3):
        self.mecanum_wheel(0, 0, 0)
        self.logout("车身停止,开始调正角度位置")
        n = 0
        while True:
            if n >= timeout:
                break
            img = self.front_camera.read()  # 读取前方相机内容
            _, angle_speed = self.cruse(img)  # 得到模型的输出
            self.logout(angle_speed)
            if (ang_max > angle_speed > ang_min) or n == 5:
                break
            self.mecanum_wheel(0, 0, angle_speed * 0.5)  # 将输出作用在巡航上
            self.logout(f"车身调整了角度{angle_speed}")
            n += 1
            time.sleep(1)

    # 获取最靠后的识别对象
    def get_closet_task(self, res):
        closet_task = None
        min_x = float("inf")
        x = 0
        dis_t = 0
        self.logout(res)
        for bbox in res:
            task, score, rect = self.label_list[int(bbox[0])], bbox[1], bbox[2:]
            if score < 0.55:  # 置信度不高的过滤掉
                continue
            if task in self.pas_task:
                continue
            middle = (rect[0] + rect[2]) / 2  # 取识别对象的中心坐标
            if self.det_direct == 1:  # 如果是不同的一侧有不同的公式
                x = 416 - middle
            elif self.det_direct == -1:
                x = middle
            if x < min_x:  # 更新出最靠前的目标
                min_x = x
                closet_task = task
                dis_t = 208 - middle  # 求出任务距离摄像头中心的距离
        dis = dis_t * self.det_direct
        # print(dis_t)
        return closet_task, dis

    # 用来输出信息和状态
    def logout(self, message):
        print(
            f"[state:{self.state}-task:{self.task_class}-dis:{self.get_dis_traveled()} m-time:{time.time() - self.start_time} s-monitoring:"
            f"{not self.monitoring_pause}-det {self.sensor.direct}:{self.sensor.res}]:{message}")

    def blind_finish_task(self):
        self.arm.do_task("grab_red_block", wait=0.4)
        time.sleep(0.5)
        self.lane_distance(distance=0.3, axis=0, speed=0.1)
        self.box.do_task("horiz")
        time.sleep(0.2)
        self.arm.do_task("store_block", wait=0.5)
        self.arm.do_task("grab_blue_block", wait=0.4)
        self.lane_distance(distance=-0.03, axis=1, speed=0.05)

        self.auto_lane_distance(distance=0.45, speed=0.15)
        self.coe = 0
        print("ok")
        self.auto_lane_distance(distance=0.4, speed=0.15)
        self.coe = 1.2

    def adjust_pos_x_by_sensor(self, timeout=7, speed=0.03):
        n = 0
        while True:
            if n >= timeout:
                break
            if self.sensor.res[1] == 0 and self.sensor.res[0] < 25:
                self.mecanum_wheel(0, 0, 0)
                self.logout("达到障碍物处")
                break
            self.lane_distance(distance=-0.005, axis=0, speed=speed)
            n += 1

    # 纯巡航模式
    def lane_mode(self):
        command = f"77 68 09 07 02 01 00 00 0A"
        command_bytes = bytes.fromhex(command)
        self.logout("开始检测是否进入纯巡航模式")
        while True:
            if self.stop_det_mode:
                break
            ret, res = self.base_moto.ser.get_answer(command_bytes)
            if ret:
                res = string_to_chunks(res.hex(), 2)
                if len(res) == 9:
                    if res[-3] != "00":
                        self.task_monitor_over = True
                        self.logout(f"任务监视线程关闭,进入纯巡航状态")
                        self.speed = 0.3
                        self.coe = 2
                        break


if __name__ == "__main__":
    my_car = MyCar()
    my_car.det_direct = 1
